## License: Apache 2.0. See LICENSE file in root directory.
## Copyright(c) 2017 Intel Corporation. All Rights Reserved.

#####################################################
##              Align Depth to Color               ##
#####################################################

# First import the library

import sys
sys.path.remove("/opt/ros/kinetic/lib/python2.7/dist-packages")
import pyrealsense2.pyrealsense2 as rs
# Import Numpy for easy array manipulation
import numpy as np
# Import OpenCV for easy image rendering
import cv2

from log.pylog import log
LOGS_INFO = log.info

# from ocr.hsv_test import hsv_split_pre
# Create a pipeline
pipeline = rs.pipeline()

# Create a config and configure the pipeline to stream
#  different resolutions of color and depth streams
config = rs.config()

# Get cv_device product line for setting a supporting resolution
pipeline_wrapper = rs.pipeline_wrapper(pipeline)
pipeline_profile = config.resolve(pipeline_wrapper)
cv_device = pipeline_profile.get_device()
device_product_line = str(cv_device.get_info(rs.camera_info.product_line))

found_rgb = False
for s in cv_device.sensors:
    if s.get_info(rs.camera_info.name) == 'RGB Camera':
        found_rgb = True
        break
if not found_rgb:
    print("The demo requires Depth camera with Color sensor")
    exit(0)

pipeline_w = 1920
pipeline_h = 1080
# pipeline_w = 1080
# pipeline_h = 680
config.enable_stream(rs.stream.color, pipeline_w, pipeline_h, rs.format.bgr8, 30)
# Start streaming
profile = pipeline.start(config)
# Getting the depth sensor's depth scale (see rs-align example for explanation)
depth_sensor = profile.get_device().first_depth_sensor()


align_to = rs.stream.color
align = rs.align(align_to)
pic_num = 0
def pre_take_pic():
    global pic_num 
    try:
        color_image =None
        for i in range(30):
            # Get frameset of color and depth
            frames = pipeline.wait_for_frames()
            # Align the depth frame to color frame
            aligned_frames = align.process(frames)
            color_frame = aligned_frames.get_color_frame()
            color_image = np.asanyarray(color_frame.get_data())
        cv2.imwrite("/home/eaibot/2022_jsjds/detection_2022/test/img/{}.jpg".format(pic_num),color_image)
        LOGS_INFO("image save as {}.jpg".format(pic_num))
        pic_num +=1
        return color_image
    except:
        print("Camera D415 Preheat Fail")
        return None

def img_split():
    pass

if __name__ == "__main__":
    for i in range(5):
        img = pre_take_pic()
        cv2.imwrite("/home/eaibot/2022_jsjds/detection_2022/test/img/{}.jpg".format(i),img)
